/*
 * KHSJoystick.cpp
 *
 *  Created on: Jan 19, 2009
 *      Author: Jesse Lovitt
 */

#include "WPILib.h"
#include "KHSJoystick.h"
#include "math.h"
#include "constants.h"
#include "mathutils.h"

/*
 * Default constructor
 */
KHSJoystick::KHSJoystick(UINT32 port):Joystick(port)
{
	printf("Finished constructing KHSJoystick Object!!\n");
}


/*
 * 
 */
KHSJoystick::KHSJoystick(UINT32 port, UINT32 numAxisTypes, UINT32 numButtonTypes):
	Joystick(port, numAxisTypes, numButtonTypes)
{
	
}


/*
 * Default destructor
 */
KHSJoystick::~KHSJoystick()
{
	
}

/*
 * Gets the angle of the joystick
 */

float KHSJoystick::get_Angle()
{
	if (GetY() == 0 && GetX() == 0)
	{
		return 0.0f;
	}
	
	else
	{
		return normalizerad(-atan2(GetY(),GetX())-PI/2);
	}
}

float KHSJoystick::getDistanceFromCenter()
{		
	return sqrt(pow(GetY(), 2) + pow(GetX(),2));
}

bool KHSJoystick::get_NormalDrive()
{
	return !GetTrigger(); 
}

float KHSJoystick::GetDriveSensitivity()
{
	return -.5f * GetThrottle() + .5f;
}
